#define CV_NO_BACKWARD_COMPATIBILITY

#include "cv.h"
#include "highgui.h"
#include <stdio.h>
#include <iostream>
#include <sstream>
#include <fstream>
#include <string>
//#include <cstdio>


#ifdef _EiC
#define WIN32
#endif


using namespace std;
using namespace cv;

void detectAndDraw( Mat& img,
                                   CascadeClassifier& cascade, CascadeClassifier& nestedCascade,
                                   double scale);
vector<Rect> detectFace( Mat& img,
                                   CascadeClassifier& cascade, vector<CascadeClassifier>& nestedCascade,
                                   double scale);


String cascadeName =
"./haarcascades/haarcascade_frontalface_alt_tree.xml";

#define BUFFER_SIZE 2048

int main( int argc, const char** argv )
{
        Mat frame, frameCopy, image;
        String inputName;
        vector<string> photoList;
        FILE * pFile;
        
        vector<string> nestedCascadeNames;
        nestedCascadeNames.push_back("./haarcascades/haarcascade_eye_tree_eyeglasses.xml");
        nestedCascadeNames.push_back("./haarcascades/haarcascade_eye.xml");
        nestedCascadeNames.push_back("./haarcascades/haarcascade_mcs_lefteye.xml");
        nestedCascadeNames.push_back("./haarcascades/haarcascade_mcs_righteye.xml");
        nestedCascadeNames.push_back("./haarcascades/haarcascade_mcs_mouth.xml");
        nestedCascadeNames.push_back("./haarcascades/haarcascade_mcs_nose.xml");
        nestedCascadeNames.push_back("./haarcascades/haarcascade_mcs_eyepair_small.xml");


        CascadeClassifier cascade;
        vector<CascadeClassifier> nestedCascades;
        double scale = 1;
        bool showResultFlag = 0;

        if( !cascade.load( cascadeName ) )
        {
                cerr << "ERROR: Could not load classifier cascade" << endl;
                cerr << "Usage: facedetect [--cascade=\"<cascade_path>\"]\n"
                        "   [--nested-cascade[=\"nested_cascade_path\"]]\n"
                        "   [--scale[=<image scale>\n"
                        "   [filename|camera_index]\n" ;
                return -1;
        }
        for ( size_t i = 0; i<nestedCascadeNames.size();i++)
        {
                CascadeClassifier nestedCascadeTmp;
                if( !nestedCascadeTmp.load( nestedCascadeNames[i] ) )
                        cerr << "WARNING: Could not load classifier cascade for nested objects" << endl;
                else{
                        nestedCascades.push_back(nestedCascadeTmp);
                }
        }
        if ( showResultFlag){
                cvNamedWindow( "result", 1 );
        }
                

        // CS229 stuff =====================================================================

		int kNumFrames = 50000;
		int *lecturerX = new int[kNumFrames];
		int *lecturerY = new int[kNumFrames];
		
		ifstream lecturer;
		lecturer.open("C:/Users/Sameer/Desktop/Stanford/cs229/EigenFace/Debug/lecturer.txt");

		for (int i = 0; i < kNumFrames; i++) {
			stringstream ss;
			string line;
			getline(lecturer,line);
			ss << line;
			ss >> lecturerX[i];  // Timestamp --> Not needed, so overwrite
			//cout << lecturerX[i] << endl;
			ss >> lecturerX[i];
			lecturerX[i] = (int) ((double)lecturerX[i] * 1920/2560);
			//cout << lecturerX[i] << endl;
			ss >> lecturerY[i];
			lecturerY[i] = (int) ((double)lecturerY[i] * 1920/2560);
			//cout << lecturerY[i] << endl;
		}
		lecturer.close();

        FILE* fresult = fopen("./test/detectResult.txt", "wt");
        CvCapture* capture = NULL;
        capture = cvCaptureFromAVI("C:/Users/Sameer/Desktop/Stanford/cs229/EigenFace/Debug/slice0_big.avi");

		int counter = 0;
		//while (cvGrabFrame(capture) && counter < 30000) {cvRetrieveFrame(capture); cout << counter++ << endl;}
        while(cvGrabFrame(capture) && counter < 50000)
        {
			//image = cvRetrieveFrame(capture);
			IplImage* image2 = cvRetrieveFrame(capture);
			 
			CvSize imageSize = cvGetSize(image2);
			int topLeftX = lecturerX[counter] - 100;
			if (topLeftX < 0) {
				topLeftX = 0;
			}
			int topLeftY = lecturerY[counter] - 150;
			if (topLeftY < 0) {
				topLeftY = 0;
			}
			int bottomRightX = topLeftX + 200;
			if (bottomRightX >= imageSize.width) {
				bottomRightX = imageSize.width - 1;
			}
			int bottomRightY = lecturerY[counter];

			// Crop the image based on lecturer position to improve speed of face detection
			cvSetImageROI(image2, cvRect(topLeftX, topLeftY, bottomRightX - topLeftX, bottomRightY - topLeftY));
			IplImage* image3 = cvCreateImage(cvGetSize(image2), image2->depth, image2->nChannels);
			
			// copy subimage 
			cvCopy(image2, image3, NULL);

			// always reset the Region of Interest 
			cvResetImageROI(image2);
			image = image3;

            int c;
            vector<Rect> faces;
            if (!image.empty())
            {
                    if (showResultFlag){
                            detectAndDraw( image, cascade, nestedCascades[0], scale );
                            c = waitKey(0);
                            if( c == 27 || c == 'q' || c == 'Q' )
                                    break;
                    }
                    else{
                            faces = detectFace( image, cascade, nestedCascades, scale);
                            fprintf(fresult,"%d",(int)faces.size());
                            /*
							for( vector<Rect>::const_iterator r = faces.begin(); r != faces.end(); r++ ){
                                    fprintf(fresult, "%d\t%d\t%d\t%d\t", r->x, r->y, r->width, r->height);
                            }
							*/
                            fprintf(fresult,"\n");
                    }
            }
			cout << "Frame no." << counter++ << endl;
			cvReleaseImage(&image3);
        }
        

        // =================================================================================



        /* assume it is a text file containing the
                        list of the image filenames to be processed - one per line */
        
        /*
        FILE* fresult = fopen("./test/detectResult.txt", "wt");
        string photoPath = "./images/";
        pFile = fopen ("./images/imgList.txt","r");
        
        while (!feof(pFile)) {
                char str[BUFFER_SIZE] = "";
                fscanf(pFile,"%s",str);
                if (strlen(str) > 0) 
                {
                        photoList.push_back(str);
                        printf("%00d.\t%s\r\n", photoList.size(), str);
                        int c;
                        vector<Rect> faces;
                        image = imread(photoPath + str ,1);
                        if( !image.empty() ){
                                if ( showResultFlag){
                                        detectAndDraw( image, cascade, nestedCascades[0], scale );
                                        c = waitKey(0);
                                        if( c == 27 || c == 'q' || c == 'Q' )
                                                break;
                                }
                                else{
                                        fprintf(fresult,"%s",str);
                                        faces = detectFace( image, cascade, nestedCascades, scale);
                                        fprintf(fresult,"\t%d\t",(int)faces.size());
                                        for( vector<Rect>::const_iterator r = faces.begin(); r != faces.end(); r++ ){
                                                fprintf(fresult, "%d\t%d\t%d\t%d\t", r->x, r->y, r->width, r->height);
                                        }
                                        fprintf(fresult,"\n");
                                }
                        }
                                
                }
        }
        
        fclose(fresult);
        fclose (pFile);
        
        */
        if ( showResultFlag){
                cvDestroyWindow("result");
        }
		cvReleaseCapture(&capture);
        fclose(fresult);
		cout << "DONE!!" << endl;

        int x;
        cin >> x;
        return 0;
}

void detectAndDraw( Mat& img,
                                   CascadeClassifier& cascade, CascadeClassifier& nestedCascade,
                                   double scale)
{
        int i = 0;
        double t = 0;
        vector<Rect> faces;
        const static Scalar colors[] =  { CV_RGB(0,0,255),
                CV_RGB(0,128,255),
                CV_RGB(0,255,255),
                CV_RGB(0,255,0),
                CV_RGB(255,128,0),
                CV_RGB(255,255,0),
                CV_RGB(255,0,0),
                CV_RGB(255,0,255)} ;
        Mat gray, smallImg( cvRound (img.rows/scale), cvRound(img.cols/scale), CV_8UC1 );

        cvtColor( img, gray, CV_BGR2GRAY );
        resize( gray, smallImg, smallImg.size(), 0, 0, INTER_LINEAR );
        equalizeHist( smallImg, smallImg );

        t = (double)cvGetTickCount();
        cascade.detectMultiScale( smallImg, faces,
                1.1, 2, 0
                //|CV_HAAR_FIND_BIGGEST_OBJECT
                //|CV_HAAR_DO_ROUGH_SEARCH
                |CV_HAAR_SCALE_IMAGE
                ,
                Size(30, 30) );
        t = (double)cvGetTickCount() - t;
        printf( "detection time = %g ms\n", t/((double)cvGetTickFrequency()*1000.) );
        for( vector<Rect>::const_iterator r = faces.begin(); r != faces.end(); r++, i++ )
        {
                Mat smallImgROI;
                vector<Rect> nestedObjects;
                Point center;
                Scalar color = colors[i%8];
                int radius;
                center.x = cvRound((r->x + r->width*0.5)*scale);
                center.y = cvRound((r->y + r->height*0.5)*scale);
                radius = cvRound((r->width + r->height)*0.25*scale);
                circle( img, center, radius, color, 3, 8, 0 );
                if( nestedCascade.empty() )
                        continue;
                smallImgROI = smallImg(*r);
                nestedCascade.detectMultiScale( smallImgROI, nestedObjects,
                        1.1, 2, 0
                        //|CV_HAAR_FIND_BIGGEST_OBJECT
                        //|CV_HAAR_DO_ROUGH_SEARCH
                        //|CV_HAAR_DO_CANNY_PRUNING
                        |CV_HAAR_SCALE_IMAGE
                        ,
                        Size(30, 30) );
                for( vector<Rect>::const_iterator nr = nestedObjects.begin(); nr != nestedObjects.end(); nr++ )
                {
                        center.x = cvRound((r->x + nr->x + nr->width*0.5)*scale);
                        center.y = cvRound((r->y + nr->y + nr->height*0.5)*scale);
                        radius = cvRound((nr->width + nr->height)*0.25*scale);
                        circle( img, center, radius, color, 3, 8, 0 );
                }
        }  
        cv::imshow( "result", img );    
}

vector<Rect> detectFace( Mat& img,
                                   CascadeClassifier& cascade, vector<CascadeClassifier>& nestedCascades,
                                   double scale)
{
        int i = 0;
        vector<Rect> faces;
        float faceScale;
        int faceLargeSize = 150;
        const static Scalar colors[] =  { CV_RGB(0,0,255),
                CV_RGB(0,128,255),
                CV_RGB(0,255,255),
                CV_RGB(0,255,0),
                CV_RGB(255,128,0),
                CV_RGB(255,255,0),
                CV_RGB(255,0,0),
                CV_RGB(255,0,255)} ;
        Mat gray, smallImg( cvRound (img.rows/scale), cvRound(img.cols/scale), CV_8UC1 );

        cvtColor( img, gray, CV_BGR2GRAY );
        resize( gray, smallImg, smallImg.size(), 0, 0, INTER_LINEAR );
        equalizeHist( smallImg, smallImg );

        cascade.detectMultiScale( smallImg, faces,
                1.1, 2, 0
                //|CV_HAAR_FIND_BIGGEST_OBJECT
                //|CV_HAAR_DO_ROUGH_SEARCH
                |CV_HAAR_SCALE_IMAGE
                ,
                Size(30, 30) );
        vector<int> effectiveFaceList;
        
        for( vector<Rect>::const_iterator r = faces.begin(); r != faces.end(); r++, i++ )
    {
        Mat smallImgROI;
        vector<Rect> nestedObjects;
        Point center;
        Scalar color = colors[i%8];
        int radius;
        center.x = cvRound((r->x + r->width*0.5)*scale);
        center.y = cvRound((r->y + r->height*0.5)*scale);
        radius = cvRound((r->width + r->height)*0.25*scale);
        circle( img, center, radius, color, 3, 8, 0 );
        
        smallImgROI = smallImg(*r);
                if (smallImgROI.cols < faceLargeSize || smallImgROI.rows < faceLargeSize)
                {
                        Mat enlargedFace(faceLargeSize, faceLargeSize, CV_8UC1);
                        faceScale = (float)faceLargeSize/smallImgROI.rows;
                        assert(smallImgROI.rows == smallImgROI.cols);
                        resize(smallImgROI,enlargedFace, enlargedFace.size(),0,0,CV_INTER_CUBIC);
                        for ( size_t n = 0; n < nestedCascades.size(); n++)
                        {
                                if( nestedCascades[n].empty() )
                                        continue;
                                nestedCascades[n].detectMultiScale( enlargedFace, nestedObjects,
                                1.1, 2, 0
                                //|CV_HAAR_FIND_BIGGEST_OBJECT
                                //|CV_HAAR_DO_ROUGH_SEARCH
                                //|CV_HAAR_DO_CANNY_PRUNING
                                |CV_HAAR_SCALE_IMAGE
                                ,
                                Size(30, 30) );
                                if ( nestedObjects.size() != 0)
                                {
                                        effectiveFaceList.push_back(i);
                                        break;
                                }

                        }
                }
                else
                {
                        for( size_t n = 0; n < nestedCascades.size(); n++)
                        {
                                if( nestedCascades[n].empty() ) 
                                {
                                        continue;
                                }
                                nestedCascades[n].detectMultiScale( smallImgROI, nestedObjects,
                                1.1, 2, 0
                                //|CV_HAAR_FIND_BIGGEST_OBJECT
                            //|CV_HAAR_DO_ROUGH_SEARCH
                             //|CV_HAAR_DO_CANNY_PRUNING
                             |CV_HAAR_SCALE_IMAGE
                             ,
                             Size(30, 30) );
                                if ( nestedObjects.size() != 0)
                                {
                                        effectiveFaceList.push_back(i);
                                        break;
                                }
                        }
                                faceScale = 1;
                }
        
        for( vector<Rect>::const_iterator nr = nestedObjects.begin(); nr != nestedObjects.end(); nr++ )
        {
            center.x = cvRound((r->x + (nr->x + nr->width*0.5)/faceScale)*scale);
            center.y = cvRound((r->y + (nr->y + nr->height*0.5)/faceScale)*scale);
            radius = cvRound((nr->width + nr->height)/faceScale*0.25*scale);
            circle( img, center, radius, color, 3, 8, 0 );
                }
                
    }  
        //cv::imshow( "result", img );   
        //int c = waitKey(0);
        vector<Rect> effectiveFaces;
        for (size_t j = 0; j < effectiveFaceList.size(); j++)
        {
                effectiveFaces.push_back(faces[effectiveFaceList[j]]);
        }
        
        return effectiveFaces;
}